#pragma once

#include <Math/Vector3.hpp>
#include <Math/Matrix4x4.hpp>
#include <Math/Matrix3x3.hpp>
#include <Graphics/Translation.hpp>
#include <Graphics/Rotation.hpp>

namespace zzz{
template <typename T>
class Transformation : public Matrix<4,4,T>
{
public:
  Transformation(){Matrix<4,4,T>::Identical();}
  Transformation(const Transformation<T> &other):Matrix<4,4,T>(other){}
  explicit Transformation(const MatrixBase<4,4,T> &other):Matrix<4,4,T>(other){}
  explicit Transformation(const Translation<T> &t)
  {
    Matrix<4,4,T>::Identical();
    T* v=Data();
    v[3]=t[0];
    v[7]=t[1];
    v[11]=t[2];
  }
  explicit Transformation(const Rotation<T> &r)
  {
    Matrix<4,4,T>::Identical();
    T* v=Data();
    v[0]=r[0];  v[1]=r[1];  v[2]=r[2];
    v[4]=r[3];  v[5]=r[4];  v[6]=r[5];
    v[8]=r[6];  v[9]=r[7];  v[10]=r[8];
  }
  Transformation(const MatrixBase<3,3,T> &r, const VectorBase<3,T> &t)
  {
    Matrix<4,4,T>::Identical();
    Set(r,t);
  }

  using Matrix<4,4,T>::operator =;
  using Matrix<4,4,T>::operator *;
  using Matrix<4,4,T>::Data;

  void Set(const MatrixBase<3,3,T> &r, const VectorBase<3,T> &t)
  {
    T* v=Data();
    v[0]=r[0];  v[1]=r[1];  v[2]=r[2];  v[3]=t[0];
    v[4]=r[3];  v[5]=r[4];  v[6]=r[5];  v[7]=t[1];
    v[8]=r[6];  v[9]=r[7];  v[10]=r[8];  v[11]=t[2];
    v[12]=0;  v[13]=0;  v[14]=0;  v[15]=1;
  }

  VectorBase<3,T> Apply(const VectorBase<3,T> &other) const
  {
    VectorBase<4,T> v(other,(T)1);
    VectorBase<4,T> res=(*this)*v;
    res/=res[3];
    return VectorBase<3,T>(res);
  }

  Transformation<T> RelativeTo(const Transformation<T> &other)
  {
    return (*this)*other.Inverted();
  }

  //Fast inverse of a rigid transform matrix
  //M=[r t]
  //  [0 1]
  //inv(M)=[r' -r'*t]
  //       [0      1]
  void Invert()
  {
    *this=Inverted();
  }

  Transformation<T> Inverted() const
  {
    const T* v=Data();
    Matrix<3,3,T> r(v[0],v[4],v[8],v[1],v[5],v[9],v[2],v[6],v[10]);
    Vector<3,T> t(-v[3],-v[7],-v[11]);
    return Transformation<T>(Rotation<T>(r),Translation<T>(r*t));
  }

   inline void ApplyGL() const;
};

template <typename T>
class GLTransformation : public Matrix<4,4,T>
{
public:
  GLTransformation(){Matrix<4,4,T>::Identical();}
  GLTransformation(const GLTransformation<T> &other):Matrix<4,4,T>(other){}
  explicit GLTransformation(const MatrixBase<4,4,T> &other):Matrix<4,4,T>(other){}
  explicit GLTransformation(const Transformation<T> &other):Matrix<4,4,T>(other.Transposed()){}
  explicit GLTransformation(const Translation<T> &t)
  {
    Matrix<4,4,T>::Identical();
    T* v=Data();
    v[12]=t[0];  v[13]=t[1];  v[14]=t[2];
  }
  explicit GLTransformation(const Rotation<T> &r)
  {
    Matrix<4,4,T>::Identical();
    T* v=Data();
    v[0]=r[0];  v[4]=r[1];  v[8]=r[2];
    v[1]=r[3];  v[5]=r[4];  v[9]=r[5];
    v[2]=r[6];  v[6]=r[7];  v[10]=r[8];
  }
  explicit GLTransformation(const Rotation<T> &r, const Translation<T> &t){Set(r,t);}

  using Matrix<4,4,T>::operator=;
  using Matrix<4,4,T>::Data;

  void operator=(const Transformation<T> &other)
  {
    *this=other.Transposed();
  }

  void Set(const Rotation<T> &r, const Translation<T> &t)
  {
    Matrix<4,4,T>::Identical();
    T* v=Data();
    v[0]=r[0];  v[4]=r[1];  v[8]=r[2];  v[12]=t[0];
    v[1]=r[3];  v[5]=r[4];  v[9]=r[5];  v[13]=t[1];
    v[2]=r[6];  v[6]=r[7];  v[10]=r[8]; v[14]=t[2];
    v[3]=0;     v[7]=0;     v[11]=0;    v[15]=1;
  }

  T& operator()(const zuint x, const zuint y) {
    return Matrix<4,4,T>::operator()(y,x);
  }

  const T& operator()(const zuint x, const zuint y) const {
    return Matrix<4,4,T>::operator()(y,x);
  }

  inline void ApplyGL() const;

  using Matrix<4,4,T>::operator *;
};
template<>
void Transformation<float>::ApplyGL() const
{
  glMultMatrixf(Transposed().Data());
}
template<>
void Transformation<double>::ApplyGL() const
{
  glMultMatrixd(Transposed().Data());
}
template<>
void GLTransformation<float>::ApplyGL() const
{
  glMultMatrixf(Data());
}
template<>
void GLTransformation<double>::ApplyGL() const
{
  glMultMatrixd(Data());
}

typedef Transformation<zfloat32> Transformationf32;
typedef Transformation<zfloat64> Transformationf64;
typedef GLTransformation<zfloat32> GLTransformationf32;
typedef GLTransformation<zfloat64> GLTransformationf64;

typedef Transformation<float> Transformationf;
typedef Transformation<double> Transformationd;
typedef GLTransformation<float> GLTransformationf;
typedef GLTransformation<double> GLTransformationd;

}
